
<!DOCTYPE html>

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta charset="utf-8" />
    <title>spatialmath.base.vectors &#8212; Spatial Maths package 0.7.0
 documentation</title>
    <link rel="stylesheet" href="../../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
    <link rel="stylesheet" type="text/css" href="../../../_static/graphviz.css" />
    <script id="documentation_options" data-url_root="../../../" src="../../../_static/documentation_options.js"></script>
    <script src="../../../_static/jquery.js"></script>
    <script src="../../../_static/underscore.js"></script>
    <script src="../../../_static/doctools.js"></script>
    <script src="../../../_static/language_data.js"></script>
    <script async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" />
   
  <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for spatialmath.base.vectors</h1><div class="highlight"><pre>
<span></span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">This modules contains functions to create and transform rotation matrices</span>
<span class="sd">and homogeneous tranformation matrices.</span>

<span class="sd">Vector arguments are what numpy refers to as ``array_like`` and can be a list,</span>
<span class="sd">tuple, numpy array, numpy row vector or numpy column vector.</span>

<span class="sd">&quot;&quot;&quot;</span>

<span class="c1"># This file is part of the SpatialMath toolbox for Python</span>
<span class="c1"># https://github.com/petercorke/spatialmath-python</span>
<span class="c1"># </span>
<span class="c1"># MIT License</span>
<span class="c1"># </span>
<span class="c1"># Copyright (c) 1993-2020 Peter Corke</span>
<span class="c1"># </span>
<span class="c1"># Permission is hereby granted, free of charge, to any person obtaining a copy</span>
<span class="c1"># of this software and associated documentation files (the &quot;Software&quot;), to deal</span>
<span class="c1"># in the Software without restriction, including without limitation the rights</span>
<span class="c1"># to use, copy, modify, merge, publish, distribute, sublicense, and/or sell</span>
<span class="c1"># copies of the Software, and to permit persons to whom the Software is</span>
<span class="c1"># furnished to do so, subject to the following conditions:</span>
<span class="c1"># </span>
<span class="c1"># The above copyright notice and this permission notice shall be included in all</span>
<span class="c1"># copies or substantial portions of the Software.</span>
<span class="c1"># </span>
<span class="c1"># THE SOFTWARE IS PROVIDED &quot;AS IS&quot;, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR</span>
<span class="c1"># IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,</span>
<span class="c1"># FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE</span>
<span class="c1"># AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER</span>
<span class="c1"># LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,</span>
<span class="c1"># OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE</span>
<span class="c1"># SOFTWARE.</span>

<span class="c1"># Contributors:</span>
<span class="c1"># </span>
<span class="c1">#     1. Luis Fernando Lara Tobar and Peter Corke, 2008</span>
<span class="c1">#     2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)</span>
<span class="c1">#     3. Peter Corke, 2020</span>

<span class="kn">import</span> <span class="nn">sys</span>
<span class="kn">import</span> <span class="nn">math</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">argcheck</span>


<span class="n">_eps</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">finfo</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float64</span><span class="p">)</span><span class="o">.</span><span class="n">eps</span>


<div class="viewcode-block" id="colvec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.colvec">[docs]</a><span class="k">def</span> <span class="nf">colvec</span><span class="p">(</span><span class="n">v</span><span class="p">):</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">v</span><span class="p">)</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">v</span><span class="p">),</span> <span class="mi">1</span><span class="p">))</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="unitvec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.unitvec">[docs]</a><span class="k">def</span> <span class="nf">unitvec</span><span class="p">(</span><span class="n">v</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create a unit vector</span>

<span class="sd">    :param v: n-dimensional vector</span>
<span class="sd">    :type v: array_like</span>
<span class="sd">    :return: a unit-vector parallel to V.</span>
<span class="sd">    :rtype: numpy.ndarray</span>
<span class="sd">    :raises ValueError: for zero length vector</span>

<span class="sd">    ``unitvec(v)`` is a vector parallel to `v` of unit length.</span>

<span class="sd">    :seealso: norm</span>

<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">v</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
    <span class="n">n</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">n</span> <span class="o">&gt;</span> <span class="mi">100</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>  <span class="c1"># if greater than eps</span>
        <span class="k">return</span> <span class="n">v</span> <span class="o">/</span> <span class="n">n</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="kc">None</span></div>


<div class="viewcode-block" id="norm"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.norm">[docs]</a><span class="k">def</span> <span class="nf">norm</span><span class="p">(</span><span class="n">v</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Norm of vector</span>

<span class="sd">    :param v: n-vector as a list, dict, or a numpy array, row or column vector</span>
<span class="sd">    :return: norm of vector</span>
<span class="sd">    :rtype: float</span>

<span class="sd">    ``norm(v)`` is the 2-norm (length or magnitude) of the vector ``v``.</span>

<span class="sd">    :seealso: unit</span>

<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span></div>


<div class="viewcode-block" id="isunitvec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.isunitvec">[docs]</a><span class="k">def</span> <span class="nf">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if vector has unit length</span>

<span class="sd">    :param v: vector to test</span>
<span class="sd">    :type v: numpy.ndarray</span>
<span class="sd">    :param tol: tolerance in units of eps</span>
<span class="sd">    :type tol: float</span>
<span class="sd">    :return: whether vector has unit length</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    :seealso: unit, isunittwist</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="nb">abs</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span> <span class="o">-</span> <span class="mi">1</span><span class="p">)</span> <span class="o">&lt;</span> <span class="n">tol</span> <span class="o">*</span> <span class="n">_eps</span></div>


<div class="viewcode-block" id="iszerovec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.iszerovec">[docs]</a><span class="k">def</span> <span class="nf">iszerovec</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if vector has zero length</span>

<span class="sd">    :param v: vector to test</span>
<span class="sd">    :type v: numpy.ndarray</span>
<span class="sd">    :param tol: tolerance in units of eps</span>
<span class="sd">    :type tol: float</span>
<span class="sd">    :return: whether vector has zero length</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    :seealso: unit, isunittwist</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span> <span class="o">&lt;</span> <span class="n">tol</span> <span class="o">*</span> <span class="n">_eps</span></div>


<div class="viewcode-block" id="isunittwist"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.isunittwist">[docs]</a><span class="k">def</span> <span class="nf">isunittwist</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if vector represents a unit twist in SE(2) or SE(3)</span>

<span class="sd">    :param v: vector to test</span>
<span class="sd">    :type v: array_like</span>
<span class="sd">    :param tol: tolerance in units of eps</span>
<span class="sd">    :type tol: float</span>
<span class="sd">    :return: whether vector has unit length</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    Vector is is intepretted as :math:`[v, \omega]` where :math:`v \in \mathbb{R}^n` and</span>
<span class="sd">    :math:`\omega \in \mathbb{R}^1` for SE(2) and :math:`\omega \in \mathbb{R}^3` for SE(3).</span>

<span class="sd">    A unit twist can be a:</span>

<span class="sd">    - unit rotational twist where :math:`|| \omega || = 1`, or</span>
<span class="sd">    - unit translational twist where :math:`|| \omega || = 0` and :math:`|| v || = 1`.</span>

<span class="sd">    :seealso: unit, isunitvec</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">v</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>

    <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">v</span><span class="p">)</span> <span class="o">==</span> <span class="mi">6</span><span class="p">:</span>
        <span class="c1"># test for SE(3) twist</span>
        <span class="k">return</span> <span class="n">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">3</span><span class="p">:</span><span class="mi">6</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">)</span> <span class="ow">or</span> <span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">3</span><span class="p">:</span><span class="mi">6</span><span class="p">])</span> <span class="o">&lt;</span> <span class="n">tol</span> <span class="o">*</span> <span class="n">_eps</span> <span class="ow">and</span> <span class="n">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">))</span>
    <span class="k">elif</span> <span class="nb">len</span><span class="p">(</span><span class="n">v</span><span class="p">)</span> <span class="o">==</span> <span class="mi">3</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">)</span> <span class="ow">or</span> <span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">&lt;</span> <span class="n">tol</span> <span class="o">*</span> <span class="n">_eps</span> <span class="ow">and</span> <span class="n">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">))</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span></div>


<div class="viewcode-block" id="isunittwist2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.isunittwist2">[docs]</a><span class="k">def</span> <span class="nf">isunittwist2</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if vector represents a unit twist in SE(2) or SE(3)</span>

<span class="sd">    :param v: vector to test</span>
<span class="sd">    :type v: array_like</span>
<span class="sd">    :param tol: tolerance in units of eps</span>
<span class="sd">    :type tol: float</span>
<span class="sd">    :return: whether vector has unit length</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    Vector is is intepretted as :math:`[v, \omega]` where :math:`v \in \mathbb{R}^n` and</span>
<span class="sd">    :math:`\omega \in \mathbb{R}^1` for SE(2) and :math:`\omega \in \mathbb{R}^3` for SE(3).</span>

<span class="sd">    A unit twist can be a:</span>

<span class="sd">    - unit rotational twist where :math:`|| \omega || = 1`, or</span>
<span class="sd">    - unit translational twist where :math:`|| \omega || = 0` and :math:`|| v || = 1`.</span>

<span class="sd">    :seealso: unit, isunitvec</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">v</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>

    <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">v</span><span class="p">)</span> <span class="o">==</span> <span class="mi">3</span><span class="p">:</span>
        <span class="c1"># test for SE(2) twist</span>
        <span class="k">return</span> <span class="n">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">)</span> <span class="ow">or</span> <span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">abs</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">&lt;</span> <span class="n">tol</span> <span class="o">*</span> <span class="n">_eps</span> <span class="ow">and</span> <span class="n">isunitvec</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">))</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span></div>


<div class="viewcode-block" id="unittwist"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.unittwist">[docs]</a><span class="k">def</span> <span class="nf">unittwist</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert twist to unit twist</span>

<span class="sd">    :param S: twist as a 6-vector</span>
<span class="sd">    :type S: array_like</span>
<span class="sd">    :param tol: tolerance in units of eps</span>
<span class="sd">    :type tol: float</span>
<span class="sd">    :return: unit twist and scalar motion</span>
<span class="sd">    :rtype: np.ndarray, shape=(6,)</span>

<span class="sd">    A unit twist is a twist where:</span>

<span class="sd">    - the rotation part has unit magnitude</span>
<span class="sd">    - if the rotational part is zero, then the translational part has unit magnitude</span>
<span class="sd">    </span>
<span class="sd">    Returns None if the twist has zero magnitude</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">s</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">6</span><span class="p">)</span>
    
    <span class="k">if</span> <span class="n">iszerovec</span><span class="p">(</span><span class="n">s</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">):</span>
        <span class="k">return</span> <span class="kc">None</span>
    
    <span class="n">v</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span>
    <span class="n">w</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="mi">3</span><span class="p">:</span><span class="mi">6</span><span class="p">]</span>

    <span class="k">if</span> <span class="n">iszerovec</span><span class="p">(</span><span class="n">w</span><span class="p">):</span>
        <span class="n">th</span> <span class="o">=</span> <span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">th</span> <span class="o">=</span> <span class="n">norm</span><span class="p">(</span><span class="n">w</span><span class="p">)</span>

    <span class="k">return</span> <span class="n">S</span> <span class="o">/</span> <span class="n">th</span></div>

<div class="viewcode-block" id="unittwist_norm"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.unittwist_norm">[docs]</a><span class="k">def</span> <span class="nf">unittwist_norm</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert twist to unit twist and norm</span>

<span class="sd">    :param S: twist as a 6-vector</span>
<span class="sd">    :type S: array_like</span>
<span class="sd">    :param tol: tolerance in units of eps</span>
<span class="sd">    :type tol: float</span>
<span class="sd">    :return: unit twist and scalar motion</span>
<span class="sd">    :rtype: tuple (np.ndarray shape=(6,), theta)</span>

<span class="sd">    A unit twist is a twist where:</span>

<span class="sd">    - the rotation part has unit magnitude</span>
<span class="sd">    - if the rotational part is zero, then the translational part has unit magnitude</span>
<span class="sd">    </span>
<span class="sd">    Returns (None,None) if the twist has zero magnitude</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">s</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">6</span><span class="p">)</span>
    
    <span class="k">if</span> <span class="n">iszerovec</span><span class="p">(</span><span class="n">s</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">):</span>
        <span class="k">return</span> <span class="p">(</span><span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">)</span>
    
    <span class="n">v</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span>
    <span class="n">w</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="mi">3</span><span class="p">:</span><span class="mi">6</span><span class="p">]</span>

    <span class="k">if</span> <span class="n">iszerovec</span><span class="p">(</span><span class="n">w</span><span class="p">):</span>
        <span class="n">th</span> <span class="o">=</span> <span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">th</span> <span class="o">=</span> <span class="n">norm</span><span class="p">(</span><span class="n">w</span><span class="p">)</span>

    <span class="k">return</span> <span class="p">(</span><span class="n">S</span> <span class="o">/</span> <span class="n">th</span><span class="p">,</span> <span class="n">th</span><span class="p">)</span></div>

<div class="viewcode-block" id="unittwist2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.unittwist2">[docs]</a><span class="k">def</span> <span class="nf">unittwist2</span><span class="p">(</span><span class="n">S</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert twist to unit twist</span>

<span class="sd">    :param S: twist as a 3-vector</span>
<span class="sd">    :type S: array_like</span>
<span class="sd">    :return: unit twist and scalar motion</span>
<span class="sd">    :rtype: tuple (unit_twist, theta)</span>

<span class="sd">    A unit twist is a twist where:</span>

<span class="sd">    - the rotation part has unit magnitude</span>
<span class="sd">    - if the rotational part is zero, then the translational part has unit magnitude</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">s</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>
    <span class="n">v</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">]</span>
    <span class="n">w</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span>

    <span class="k">if</span> <span class="n">iszerovec</span><span class="p">(</span><span class="n">w</span><span class="p">):</span>
        <span class="n">th</span> <span class="o">=</span> <span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">th</span> <span class="o">=</span> <span class="n">norm</span><span class="p">(</span><span class="n">w</span><span class="p">)</span>

    <span class="k">return</span> <span class="p">(</span><span class="n">S</span> <span class="o">/</span> <span class="n">th</span><span class="p">,</span> <span class="n">th</span><span class="p">)</span></div>


<div class="viewcode-block" id="angdiff"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.vectors.angdiff">[docs]</a><span class="k">def</span> <span class="nf">angdiff</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="n">b</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Angular difference</span>

<span class="sd">    :param a: angle in radians</span>
<span class="sd">    :type a: scalar or array_like</span>
<span class="sd">    :param b: angle in radians</span>
<span class="sd">    :type b: scalar or array_like</span>
<span class="sd">    :return: angular difference a-b</span>
<span class="sd">    :rtype: scalar or array_like</span>

<span class="sd">    - If ``a`` and ``b`` are both scalars, the result is scalar</span>
<span class="sd">    - If ``a`` is array_like, the result is a vector a[i]-b</span>
<span class="sd">    - If ``a`` is array_like, the result is a vector a-b[i]</span>
<span class="sd">    - If ``a`` and ``b`` are both vectors of the same length, the result is a vector a[i]-b[i]</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">mod</span><span class="p">(</span><span class="n">a</span> <span class="o">-</span> <span class="n">b</span> <span class="o">+</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span><span class="p">,</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span><span class="p">)</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span></div>


<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">&#39;__main__&#39;</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="kn">import</span> <span class="nn">pathlib</span>
    <span class="kn">import</span> <span class="nn">os.path</span>

    <span class="n">exec</span><span class="p">(</span><span class="nb">open</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">absolute</span><span class="p">(),</span> <span class="s2">&quot;test_transforms.py&quot;</span><span class="p">))</span><span class="o">.</span><span class="n">read</span><span class="p">())</span>
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper">
<p class="logo">
  <a href="../../../index.html">
    <img class="logo" src="../../../_static/../../../../figs/icon.png" alt="Logo"/>
    
  </a>
</p>



<p class="blurb">Spatial maths and geometry for Python</p>




<p>
<iframe src="https://ghbtns.com/github-btn.html?user=petercorke&repo=spatialmath-python&type=watch&count=true&size=large&v=2"
  allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>
</p>





<h3>Navigation</h3>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../../intro.html">Introduction</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../spatialmath.html">Classes and functions</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../indices.html">Indices</a></li>
</ul>

<div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../../index.html">Documentation overview</a><ul>
  <li><a href="../../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3 id="searchlabel">Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../../search.html" method="get">
      <input type="text" name="q" aria-labelledby="searchlabel" />
      <input type="submit" value="Go" />
    </form>
    </div>
</div>
<script>$('#searchbox').show(0);</script>








        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2020, Peter Corke.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 2.4.4</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>